#include <stdio.h>
#include <stdlib.h>
#include "DSP28x_Project.h"
#include "servos_motor.h"
#include "imu.h"
#include "mnm.h"
#include "sci.h"
#include "command.h"

#define true 1
#define false 0
#define MAX_SCI_MSG_ARRAY_SIZE 255


void InitI2C();
void InitIMU();
/*
void InitGPS();
void InitMNM();

void InitSCI();
void InitADC();
*/


int main() {

	Uint16 IMU_offsets[6] = {0,0,0,0,0,0};

//	byte currentControlValues;
//	byte SCI_MSG_Array[MAX_SCI_MSG_ARRAY_SIZE];
//	Uint16 SCI_MSG_Index = 0;
//	Uint16 SCI_Inactivity_Count = 0;
//	unsigned long int i;
	// initialize system control
	// "This example function is found in the DSP2803x_SysCtrl.c file."
	InitSysCtrl();

	InitPWM();
	scia_init();

	//ESCAPE (FROM BYTE 1)  0x7E, 0x7D, 0x11, 0x13 -> 0x7D , (oldvalue)xor(0x20)
	byte msg[] = {'h','i'};
	InitI2C();
	InitIMU();
	scia_msg(msg, 2);


	IMU_read_offsets(IMU_offsets);
	for(;;) {

		// main flight loop
		/*
		if(SCI_ll_front != NULL && SCI_ll_front->read == false) {
			SCI_MSG_Array[SCI_MSG_Index] = SCI_ll_front->message;
			SCI_ll_front->read = true;
			SCI_MSG_Index++;
			SCI_Inactivity_Count = 0;
		} else {
			SCI_Inactivity_Count++;
		}

		if(SCI_MSG_Index > 3 && SCI_MSG_Array[SCI_MSG_Index - 1] == 0 && SCI_MSG_Array[SCI_MSG_Index - 2] == 0) {
			switch(SCI_MSG_Array[0]) {
			case COMMAND_ROUTE:
			case COMMAND_RETURN:
			case COMMAND_ELAND:
			case COMMAND_STOP:
			case COMMAND_DOCKINGOFFSETS:
			case COMMAND_DOCKNOTFOUND:
			case COMMAND_CONTROL:
				currentControlValues = SCI_MSG_Array[1] ^ 0xFF;
			}
			SCI_MSG_Index = 0;
		}

		if(currentControlValues != 0 && SCI_Inactivity_Count = 1000) {
			// send query byte
		} else if(currentControlValues != 0 && SCI_Inactivity_Count = 2000) {
			currentControlValues == 0;
		}
		*/
		//InitIMU_Offsets();
		//j++;
	}
	return 0;
}

void InitI2C() {

	   EALLOW;

	/* Enable internal pull-up for the selected pins */
	// Pull-ups can be enabled or disabled disabled by the user.
	// This will enable the pullups for the specified pins.
	// Comment out other unwanted lines.

	GpioCtrlRegs.GPAPUD.bit.GPIO28 = 0;    // Enable pull-up for GPIO28 (SDAA)
	GpioCtrlRegs.GPAPUD.bit.GPIO29 = 0;	   // Enable pull-up for GPIO29 (SCLA)

	//	GpioCtrlRegs.GPBPUD.bit.GPIO32 = 0;    // Enable pull-up for GPIO32 (SDAA)
	//	GpioCtrlRegs.GPBPUD.bit.GPIO33 = 0;	   // Enable pull-up for GPIO33 (SCLA)

	/* Set qualification for selected pins to asynch only */
	// This will select asynch (no qualification) for the selected pins.
	// Comment out other unwanted lines.

	GpioCtrlRegs.GPAQSEL2.bit.GPIO28 = 3;  // Asynch input GPIO28 (SDAA)
	GpioCtrlRegs.GPAQSEL2.bit.GPIO29 = 3;  // Asynch input GPIO29 (SCLA)

	//	GpioCtrlRegs.GPBQSEL1.bit.GPIO32 = 3;  // Asynch input GPIO32 (SDAA)
	//  GpioCtrlRegs.GPBQSEL1.bit.GPIO33 = 3;  // Asynch input GPIO33 (SCLA)

	/* Configure I2C pins using GPIO regs*/
	// This specifies which of the possible GPIO pins will be I2C functional pins.
	// Comment out other unwanted lines.

	GpioCtrlRegs.GPAMUX2.bit.GPIO28 = 2;   // Configure GPIO28 for SDAA operation
	GpioCtrlRegs.GPAMUX2.bit.GPIO29 = 2;   // Configure GPIO29 for SCLA operation

	//	GpioCtrlRegs.GPBMUX1.bit.GPIO32 = 1;   // Configure GPIO32 for SDAA operation
	//	GpioCtrlRegs.GPBMUX1.bit.GPIO33 = 1;   // Configure GPIO33 for SCLA operation

	EDIS;

	// Initialize I2C
	I2caRegs.I2CSAR = 0x0068;		// Slave address - IMU control code

	I2caRegs.I2CPSC.all = 5;		// Prescaler - need 7-12 Mhz on module clk
	I2caRegs.I2CCLKL = 10;			// NOTE: must be non zero
	I2caRegs.I2CCLKH = 5;			// NOTE: must be non zero
//	I2caRegs.I2CIER.all = 0x24;		// Enable SCD & ARDY interrupts

	I2caRegs.I2CMDR.all = 0x0020;	// Take I2C out of reset
									// Stop I2C when suspended

	I2caRegs.I2CFFTX.all = 0x6000;	// Enable FIFO mode and TXFIFO
	I2caRegs.I2CFFRX.all = 0x2040;	// Enable RXFIFO, clear RXFFINT,

	return;
}


//===========================================================================
// No more.
//===========================================================================


